#include "mc_task.h"
#include "mc_foc.h"
#include "stdio.h"
#include "mc_control.h"

Motor_t motor = {0};

extern TIM_HandleTypeDef htim1;
extern ADC_HandleTypeDef hadc1;
extern ADC_HandleTypeDef hadc2;
extern SPI_HandleTypeDef hspi1;
extern ADCParam_t motor_adc_param;
extern McFocParam_t motor_foc_param;
extern EncoderData_t motor_enc_param;
extern PidHandle_t id_pid;
extern PidHandle_t iq_pid;


uint8_t mt6701_rx_data[3];

#define TEST_MODE 0

void mc_nvic_init(void)
{
    /* ADC_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(ADC1_2_IRQn, 3, 0);
    HAL_NVIC_EnableIRQ(ADC1_2_IRQn);
    /* DMA1_Channel2_IRQn interrupt configuration */
    HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 4, 0);
    HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
    /* USART2 interrupt Init */
    HAL_NVIC_SetPriority(USART2_IRQn, 8, 0);
    HAL_NVIC_EnableIRQ(USART2_IRQn);
}

void mc_timer_init(void)
{
    //initial pwm output timmer
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
    //start adc inject it
    HAL_ADCEx_InjectedStart_IT(&hadc1);
    HAL_ADCEx_InjectedStart(&hadc2);
    //start time base 
    HAL_TIM_Base_Start(&htim1);
}

//start spi dma translate
void encoder_init(void)
{
    HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);//pull low spi nss
    HAL_SPI_TransmitReceive_DMA(&hspi1, mt6701_rx_data, mt6701_rx_data, 3);//start spi receive dma 
}

/**
  * @brief  initial motor drv's parameter and eht encoder offset of foc control
  * you need to close adc befor do encoder alignment incase jump into adc nvic function
  * @param  motor: motor structour
  * @retval null
  */
void motor_param_init(Motor_t *motor)
{

    //motor parameter init
    motor->adc_content = motor_adc_param;
    motor->encoder_content = motor_enc_param;
    motor->mc_foc_param = motor_foc_param;

    //encoder alignment
    CLEAR_BIT(ADC1->CR2, ADC_CR2_ADON); // disable adc for motor encoder alignment
    CLEAR_BIT(ADC2->CR2, ADC_CR2_ADON); // disable adc for motor encoder alignment

    TIM1->CCR1 = 1200;
    TIM1->CCR2 = 0;
    TIM1->CCR3 = 0;
    TIM1->CCR4 = 1200;
    HAL_Delay(200);
    motor->encoder_content.encoder_offset =(- motor->encoder_content.e_angle);

    SET_BIT(ADC1->CR2, ADC_CR2_ADON); // enable adc
    SET_BIT(ADC2->CR2, ADC_CR2_ADON);

    //set adc inject channel
    motor->adc_content.adc1->JSQR = (uint32_t)(ADC_CHANNEL_0) << 15;//set inject channel of adc1
    motor->adc_content.adc2->JSQR = (uint32_t)(ADC_CHANNEL_1) << 15;//set inject channel of adc2
    WRITE_REG(ADC1->SR, ~ADC_SR_JEOC);  // clear JEOC flag
    SET_BIT(ADC1->CR1, ADC_CR1_JEOCIE); // enable adc inject interrup
}


void mc_init(Motor_t *motor)
{
    encoder_init();
    mc_timer_init();
    mc_nvic_init();
    encoder_init();
    motor_param_init(motor);
}



void ADC1_2_IRQHandler(void)
{
    if (READ_BIT(motor.adc_content.adc1->SR, ADC_SR_JEOC) == (ADC_SR_JEOC))
    {
        __HAL_ADC_DISABLE_IT(&hadc1, ADC_IT_JEOC);
        ADC1->SR &= ~(uint32_t)(ADC_SR_JEOC | ADC_SR_JSTRT);
        // FOC FUNCTION
        if(motor.mc_foc_param.control_mod != control_null)
        {
            foc_control(&motor);
        }
        __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
        SET_BIT(TIM1->CCER, TIM_CCER_CC4E);
    }
}

uint8_t calculate_crc(uint32_t data)
{
    uint8_t crc = 0;
    uint32_t polynomial = 0x43; // binnary format of (X^6 + X + 1)

    // (D[13:0] 和 Mg[3:0])
    for (int i = 17; i >= 0; i--)
    {
        uint8_t bit = (data >> i) & 1;
        crc <<= 1;
        if ((crc >> 6) ^ bit)
            crc ^= polynomial;
        crc &= 0x3F;
    }
    return crc;
}

void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
    if (hspi->Instance == SPI1)
    {
        
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);
        HAL_SPI_DMAStop(hspi);
        int32_t angle_raw = (mt6701_rx_data[1] >> 2) | (mt6701_rx_data[0] << 6);

        uint8_t crc_raw = mt6701_rx_data[2] & ((1 << 6) - 1);
        uint32_t crc_data = (mt6701_rx_data[0] << 16 | mt6701_rx_data[1] << 8 | mt6701_rx_data[2]) >> 6;
        if (calculate_crc(crc_data) != crc_raw)
        {
            HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
            HAL_SPI_TransmitReceive_DMA(&hspi1, mt6701_rx_data, mt6701_rx_data, 3);
            return;
        }
        HAL_SPI_DMAResume(hspi);
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
        HAL_SPI_TransmitReceive_DMA(&hspi1, mt6701_rx_data, mt6701_rx_data, 3);
        motor.encoder_content.encoder_count_pre[0] = angle_raw;
        motor.encoder_content.e_angle = (((angle_raw * MOTOR_POLESPAIR)%16384) - 8192) << 2;
        motor.encoder_content.e_angle_act = motor.encoder_content.e_angle + motor.encoder_content.encoder_offset;
    }
}

void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
{
    printf("HAL_SPI_ErrorCallback:%d\n", (int)hspi->ErrorCode);
    if (hspi->Instance == SPI1)
    {
    	HAL_SPI_TransmitReceive_DMA(&hspi1, mt6701_rx_data, mt6701_rx_data, 3);
    }
    return;
}
